copp\copp\copp3/
interpolation.rs

1//! Interpolation and profile-conversion utilities for third-order path parameterization.
2//!
3//! # Method identity
4//! This module serves both:
5//! - **Time-Optimal Path Parameterization (TOPP3)** workflows,
6//! - **Convex-Objective Path Parameterization (COPP3)** workflows.
7//!
8//! # Scope
9//! This module provides deterministic conversions between:
10//! - node profiles `a(s) = \dot{s}^2` and `b(s) = \ddot{s}` sampled on stations,
11//! - time mapping `t(s)`,
12//! - inverse sampling `s(t)`.
13//!
14//! # Conventions
15//! - Path grid uses station samples `s[0..=n]`.
16//! - Both `a` and `b` are node-based in TOPP3/COPP3 (`a.len() == b.len() == s.len()`).
17//! - `num_stationary = (head, tail)` indicates stationary boundary counts at start/end.
18//!
19//! # Example
20//! The example below converts a third-order profile from station samples to
21//! cumulative time and then samples the inverse map `s(t)`.
22//!
23//! ```rust
24//! # fn main() -> Result<(), copp::diag::CoppError> {
25//! use copp::InterpolationMode;
26//! use copp::solver::topp3_lp::{s_to_t_topp3, t_to_s_topp3, Topp3Profile};
27//!
28//! let s = [0.0, 0.5, 1.0];
29//! let profile = Topp3Profile::new(
30//!     vec![1.0, 1.0, 1.0],
31//!     vec![0.0, 0.0, 0.0],
32//!     (0, 0),
33//! );
34//!
35//! let (_t_final, t_s) = s_to_t_topp3(&s, profile.as_parts(), 0.0)?;
36//! let s_t = t_to_s_topp3(
37//!     &s,
38//!     profile.as_parts(),
39//!     &t_s,
40//!     InterpolationMode::UniformTimeGrid(0.0, 0.25, true),
41//! )?;
42//!
43//! assert_eq!(s_t.first().copied(), Some(0.0));
44//! assert_eq!(s_t.last().copied(), Some(1.0));
45//! # Ok(())
46//! # }
47//! ```
48
49use crate::copp::InterpolationMode;
50use crate::copp::copp3::{Topp3ProfileMut, Topp3ProfileRef};
51use crate::diag::{
52    CoppError, check_input_len_at_least, check_input_len_equal, check_input_non_negative,
53    check_input_not_empty, check_input_not_nan_infinite, check_input_slice_non_negative,
54    check_input_slice_not_nan_infinite, check_input_strictly_increasing,
55};
56use crate::math::numerical::{EPS_ZERO, solve_2x2};
57use itertools::izip;
58
59/// Compute cumulative time profile `t(s)` from a TOPP3/COPP3 profile.
60///
61/// # Semantics
62/// - `t_s[i]` is the time at station `s[i]`.
63/// - initial condition is `t_s[0] = t0`.
64/// - returns `(t_final, t_s)` where `t_final == *t_s.last().unwrap()`.
65///
66/// # Input contract
67/// - valid when `s.len() >= 2 + profile.2.0 + profile.2.1`;
68/// - requires `profile.0.len() == s.len()` and `profile.1.len() == s.len()`;
69/// - all inputs must contain only finite values;
70/// - `s` must be strictly increasing.
71///
72/// # Returns
73/// Returns `(t_final, t_s)` where `t_s[i]` is cumulative time at `s[i]`.
74///
75/// # Errors
76/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, stationary counts,
77/// monotonicity, positivity, or numeric finiteness requirements are violated.
78///
79/// # Contract
80/// - `t_s.len() == s.len()` on valid input.
81/// - `t_s[0] == t0` on valid input.
82pub fn s_to_t_topp3(
83    s: &[f64],
84    profile: Topp3ProfileRef<'_>,
85    t0: f64,
86) -> Result<(f64, Vec<f64>), CoppError> {
87    check_topp3_sab("s_to_t_topp3", s, profile)?;
88    check_input_not_nan_infinite("s_to_t_topp3", "t0", t0)?;
89    let (a, b, num_stationary) = profile;
90    let mut t_s = Vec::<f64>::with_capacity(s.len()); // t_s[i] = t(s[i]), begin from t0
91    let mut t_prev = t0;
92    let n = s.len() - 1;
93    t_s.push(t_prev);
94    if num_stationary.0 > 0 {
95        let s0 = s.first().unwrap();
96        t_s.resize(1 + num_stationary.0, t_prev);
97        for (t_curr, a_curr, s_curr) in izip!(t_s.iter_mut(), a.iter(), s.iter()).skip(1) {
98            *t_curr += 3.0 * (s_curr - s0) / a_curr.sqrt();
99        }
100        t_prev = *t_s.last().unwrap();
101    }
102    for (s_pair, b_pair, a_curr) in izip!(s.windows(2), b.windows(2), a.iter())
103        .skip(num_stationary.0)
104        .take(n - num_stationary.0 - num_stationary.1)
105    {
106        t_prev += integral_rsrqp(
107            *a_curr,
108            2.0 * b_pair[0],
109            (b_pair[1] - b_pair[0]) / (s_pair[1] - s_pair[0]),
110            0.0,
111            s_pair[1] - s_pair[0],
112        );
113        t_s.push(t_prev);
114    }
115    if num_stationary.1 > 0 {
116        let s_final = s.last().unwrap();
117        let t_final =
118            t_prev + 3.0 * (s_final - s[n - num_stationary.1]) / a[n - num_stationary.1].sqrt();
119        t_s.resize(s.len(), t_final);
120        if num_stationary.1 > 1 {
121            for (t_curr, a_curr, s_curr) in izip!(t_s.iter_mut(), a.iter(), s.iter())
122                .rev()
123                .skip(1)
124                .take(num_stationary.1 - 1)
125            {
126                *t_curr += 3.0 * (s_curr - s_final) / a_curr.sqrt();
127            }
128        }
129    }
130
131    let t_final = *t_s.last().unwrap();
132    if !t_final.is_finite() || t_s.iter().any(|value| !value.is_finite()) {
133        return Err(CoppError::InvalidInput(
134            "s_to_t_topp3".into(),
135            "computed time profile contains NaN or infinity".into(),
136        ));
137    }
138    check_input_strictly_increasing("s_to_t_topp3", "t_s", &t_s)?;
139    Ok((t_final, t_s))
140}
141
142/// Compute definite integral of reciprocal-square-root quadratic polynomial:
143/// $$dt = \int_{x_{left}}^{x_{right}} \frac{dx}{\sqrt{c_0 + c_1 x + c_2 x^2}}.$$
144fn integral_rsrqp(c0: f64, c1: f64, c2: f64, x_left: f64, x_right: f64) -> f64 {
145    if c2 > f64::EPSILON {
146        let func = |x: f64| x + 0.5 * c1 / c2 + (x * x + (c1 * x + c0) / c2).sqrt();
147        (func(x_right).abs().ln() - func(x_left).abs().ln()) / c2.sqrt()
148    } else if c2 < -f64::EPSILON {
149        let delta = c1 * c1 - 4.0 * c2 * c0;
150        if delta > 0.0 {
151            let func = |x: f64| (-2.0 * c2 * x - c1) / delta.sqrt();
152            (func(x_right).asin() - func(x_left).asin()) / (-c2).sqrt()
153        } else {
154            f64::INFINITY
155        }
156    } else if c1.abs() > f64::EPSILON {
157        // Dt = \int_{xl}^{xr} dx/sqrt(C1*x+C0)
158        2.0 / c1 * ((c1 * x_right + c0).sqrt() - (c1 * x_left + c0).sqrt())
159    } else if c0.abs() > f64::EPSILON {
160        // Dt = \int_{xl}^{xr} dx/sqrt(C0)
161        (x_right - x_left) / c0.sqrt()
162    } else {
163        f64::INFINITY
164    }
165}
166
167/// Interpolate inverse mapping `s(t)` from a TOPP3/COPP3 profile and sampled `t(s)`.
168///
169/// # Modes
170/// - [`UniformTimeGrid`](crate::InterpolationMode::UniformTimeGrid)`(t0, dt, include_final)`: generate uniform time samples;
171/// - `NonUniformTimeGrid(t_sample)`: use caller-provided increasing samples.
172///
173/// # Input contract
174/// - requires `s.len() >= 2`, profile slice lengths equal to `s.len()`, and `t_s.len() == s.len()`;
175/// - requires `t_s` strictly increasing;
176/// - all profile and time-grid values must be finite.
177///
178/// # Output semantics
179/// - output length matches requested sample count in each mode;
180/// - for out-of-range time samples, output entries are `NaN`.
181///
182/// # Returns
183/// Returns sampled `s(t)` values under the requested interpolation `mode`.
184///
185/// # Errors
186/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, stationary counts,
187/// monotonicity, positivity, or numeric finiteness requirements are violated.
188///
189/// # Contract
190/// - preserves caller time-sample ordering.
191/// - malformed input is reported as [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput).
192pub fn t_to_s_topp3(
193    s: &[f64],
194    profile: Topp3ProfileRef<'_>,
195    t_s: &[f64],
196    mode: InterpolationMode<'_>,
197) -> Result<Vec<f64>, CoppError> {
198    check_topp3_sab("t_to_s_topp3", s, profile)?;
199    check_input_len_equal(
200        "t_to_s_topp3",
201        "`t_s.len()`",
202        t_s.len(),
203        "`s.len()`",
204        s.len(),
205    )?;
206    check_input_slice_not_nan_infinite("t_to_s_topp3", "t_s", t_s)?;
207    check_input_strictly_increasing("t_to_s_topp3", "t_s", t_s)?;
208    match mode {
209        InterpolationMode::UniformTimeGrid(t0, dt, include_final) => {
210            check_input_not_nan_infinite("t_to_s_topp3", "t0", t0)?;
211            check_input_not_nan_infinite("t_to_s_topp3", "dt", dt)?;
212            if dt <= 0.0 {
213                return Err(CoppError::InvalidInput(
214                    "t_to_s_topp3".into(),
215                    format!("`dt` = {dt} must be positive"),
216                ));
217            }
218            // num_t * dt + t0 <= t_final
219            let num_t = ((t_s.last().unwrap() - t0) / dt).floor() as usize;
220            let mut s_t = t_to_s_topp3_core(
221                s,
222                profile,
223                t_s,
224                (0..num_t).map(|i| t0 + i as f64 * dt),
225                num_t,
226            );
227            if include_final {
228                let flag = if s_t.is_empty() {
229                    t0 <= *t_s.last().unwrap()
230                } else {
231                    *s_t.last().unwrap() < *s.last().unwrap()
232                };
233                if flag {
234                    s_t.push(*s.last().unwrap());
235                }
236            }
237            Ok(s_t)
238        }
239        InterpolationMode::NonUniformTimeGrid(t_sample) => {
240            check_input_not_empty("t_to_s_topp3", "`t_sample`", t_sample.len())?;
241            check_input_slice_not_nan_infinite("t_to_s_topp3", "t_sample", t_sample)?;
242            check_input_strictly_increasing("t_to_s_topp3", "t_sample", t_sample)?;
243            Ok(t_to_s_topp3_core(
244                s,
245                profile,
246                t_s,
247                t_sample.iter().cloned(),
248                t_sample.len(),
249            ))
250        }
251    }
252}
253
254/// Core inverse interpolation kernel for [`t_to_s_topp3`](crate::solver::topp3_socp::t_to_s_topp3).
255///
256/// The public wrapper validates dimensions, finiteness, station ordering, and
257/// sample ordering before calling this routine. This core then walks the time
258/// samples once and maps each sample into the corresponding station interval.
259fn t_to_s_topp3_core(
260    s: &[f64],
261    profile: Topp3ProfileRef<'_>,
262    t_s: &[f64],
263    mut t_sample: impl Iterator<Item = f64>,
264    len_t_sample: usize,
265) -> Vec<f64> {
266    let (a, b, num_stationary) = profile;
267    // Map t to s
268    let &t_start = t_s.first().unwrap();
269    let &t_final = t_s.last().unwrap();
270    let mut s_t = Vec::<f64>::with_capacity(len_t_sample + 1); // s_t[i] = s(t[i])
271    let Some(mut t_curr) = t_sample.next() else {
272        return vec![];
273    };
274    while t_curr < t_start {
275        s_t.push(f64::NAN);
276        let Some(t) = t_sample.next() else {
277            return s_t;
278        };
279        t_curr = t;
280    }
281
282    if num_stationary.0 > 0 {
283        let s0 = s.first().unwrap();
284        let a_stationary = a[num_stationary.0];
285        let t_stationary = t_s[num_stationary.0];
286        let d3u_over_6 =
287            a_stationary.sqrt() * a_stationary / (27.0 * (s[num_stationary.0] - s0).powi(2));
288        while t_curr <= t_stationary {
289            s_t.push(s0 + d3u_over_6 * (t_curr - t_start).powi(3));
290            let Some(t) = t_sample.next() else {
291                return s_t;
292            };
293            t_curr = t;
294        }
295    }
296
297    for (s_pair, &a_curr, b_pair, t_pair) in
298        izip!(s.windows(2), a.iter(), b.windows(2), t_s.windows(2))
299            .skip(num_stationary.0)
300            .take(s.len() - num_stationary.0 - num_stationary.1 - 1)
301    {
302        while t_curr <= t_pair[1] {
303            s_t.push(
304                s_pair[0]
305                    + inverse_rsrqp(
306                        a_curr,
307                        2.0 * b_pair[0],
308                        (b_pair[1] - b_pair[0]) / (s_pair[1] - s_pair[0]),
309                        0.0,
310                        t_curr - t_pair[0],
311                    ),
312            );
313            let Some(t) = t_sample.next() else {
314                return s_t;
315            };
316            t_curr = t;
317        }
318    }
319
320    if num_stationary.1 > 0 {
321        let s_final = s.last().unwrap();
322        let a_stationary = a[s.len() - num_stationary.1 - 1];
323        let d3u_over_6 = a_stationary.sqrt() * a_stationary
324            / (27.0 * (s_final - s[s.len() - num_stationary.1 - 1]).powi(2));
325        while t_curr <= t_final {
326            s_t.push(s_final + d3u_over_6 * (t_curr - t_final).powi(3));
327            let Some(t) = t_sample.next() else {
328                return s_t;
329            };
330            t_curr = t;
331        }
332    }
333
334    s_t.push(f64::NAN);
335    while t_sample.next().is_some() {
336        s_t.push(f64::NAN);
337    }
338    s_t
339}
340
341/// Solve `x_right` from
342/// $$dt = \int_{x_{left}}^{x_{right}} \frac{dx}{\sqrt{c_0 + c_1 x + c_2 x^2}}.$$
343fn inverse_rsrqp(c0: f64, c1: f64, c2: f64, x_left: f64, dt: f64) -> f64 {
344    if dt == 0.0 {
345        return x_left;
346    }
347    let delta = c1 * c1 - 4.0 * c2 * c0;
348    if c2 > f64::EPSILON {
349        let mu = (c2.sqrt() * dt
350            + (x_left + 0.5 * c1 / c2 + (x_left * x_left + (c1 * x_left + c0) / c2).sqrt())
351                .abs()
352                .ln())
353        .exp();
354        let xr1 = -0.5 * c1 / c2 + 0.5 * (mu + delta / (4.0 * c2 * c2 * mu));
355        let xr2 = -0.5 * c1 / c2 - 0.5 * (mu + delta / (4.0 * c2 * c2 * mu));
356        let mut flag1 = true;
357        let mut flag2 = true;
358        if dt > 0.0 {
359            flag1 &= xr1 > x_left;
360            flag2 &= xr2 > x_left;
361        } else {
362            flag1 &= xr1 < x_left;
363            flag2 &= xr2 < x_left;
364        }
365        if flag1 && flag2 {
366            let dt1 = integral_rsrqp(c0, c1, c2, x_left, xr1);
367            let dt2 = integral_rsrqp(c0, c1, c2, x_left, xr2);
368            if (dt1 - dt).abs() < (dt2 - dt).abs() {
369                xr1
370            } else {
371                xr2
372            }
373        } else if flag1 {
374            xr1
375        } else if flag2 {
376            xr2
377        } else {
378            f64::INFINITY
379        }
380    } else if c2 < -f64::EPSILON {
381        (c1 + delta.sqrt()
382            * ((-c2).sqrt() * dt + ((-2.0 * c2 * x_left - c1) / delta.sqrt()).asin()).sin())
383            / (-2.0 * c2)
384    } else if c1.abs() > f64::EPSILON {
385        ((0.5 * c1 * dt + (c1 * x_left + c0).sqrt()).powi(2) - c0) / c1
386    } else if c0.abs() > f64::EPSILON {
387        c0.sqrt() * dt + x_left
388    } else {
389        f64::INFINITY
390    }
391}
392
393/// Post-process a mutable `(a, b)` profile so that interpolated `a(s)` stays strictly positive per interval.
394///
395/// This is a numerical safety utility for downstream timing integration on
396/// profiles that may be very close to zero due to finite precision.
397///
398/// # Returns
399/// Returns `true` when in-place adjustment succeeds, otherwise `false`.
400///
401/// # Errors
402/// Returns [`CoppError::InvalidInput`](crate::diag::CoppError::InvalidInput) when dimensions, station ordering, profile
403/// positivity, stationary counts, or numeric finiteness requirements are violated.
404///
405/// # Contract
406/// - requires `a.len() == b.len() == s.len()` and `s.len() >= 4`;
407/// - requires endpoint `a` values to be nonnegative.
408pub fn force_positive_a(
409    profile: Topp3ProfileMut<'_>,
410    s: &[f64],
411    a_min: f64,
412) -> Result<bool, CoppError> {
413    let (a, b, num_stationary) = profile;
414    let n = s.len();
415    if a.len() != n || b.len() != n {
416        return Err(CoppError::InvalidInput(
417            "force_positive_a".into(),
418            format!(
419                "`a.len()` = {} and `b.len()` = {} must equal `s.len()` = {}",
420                a.len(),
421                b.len(),
422                n
423            ),
424        ));
425    }
426    if n < 4 {
427        return Err(CoppError::InvalidInput(
428            "force_positive_a".into(),
429            format!("`s.len()` = {n} must be at least 4"),
430        ));
431    }
432    check_stationary_counts("force_positive_a", n, num_stationary)?;
433    check_input_slice_not_nan_infinite("force_positive_a", "s", s)?;
434    check_input_slice_non_negative("force_positive_a", "a", a)?;
435    check_input_slice_not_nan_infinite("force_positive_a", "b", b)?;
436    check_input_non_negative("force_positive_a", "a_min", a_min)?;
437    check_input_strictly_increasing("force_positive_a", "s", s)?;
438    // Now we have a(s[i]) >= 0, and we would like to modify a(s) > 0 for s in (s[i], s[i+1]) if a(s) can be negative for some s in (s[i], s[i+1]).
439    let mut flag_succeed = true;
440    for i in (num_stationary.0 + 1)..(n - 2 - num_stationary.1) {
441        // Consider a[i-1], a[i], a[i+1], a[i+2]
442        let b1 = b[i];
443        let b2 = b[i + 1];
444        if b1 < 0.0 && b2 > 0.0 {
445            // a(s) = a[i] + 2 * b[i] * (s - s[i]) + (b[i+1] - b[i]) / ds1 * (s - s[i])^2
446            // b[i] ^ 2 < a[i] * (b[i+1] - b[i]) / ds1 should hold
447            // b[i] ^ 2 * ds1 < a[i] * (b[i+1] - b[i]) should hold
448            let s1 = s[i];
449            let s2 = s[i + 1];
450            let ds1 = s2 - s1;
451            let a1 = a[i];
452            let amin = a_min.max(a1.min(a[i + 1]));
453            let amin = if amin > 10.0 * EPS_ZERO {
454                0.1 * amin
455            } else if amin > EPS_ZERO {
456                EPS_ZERO
457            } else {
458                amin
459            };
460            let da = a1 - amin;
461            let db = b2 - b1;
462            if b1 * b1 * ds1 >= da * db {
463                // a(s) <= 0 holds in (s[i], s[i+1])
464                // We add c0 on (s[i-1],s[i+2]), c1 on (s[i],s[i+2]), and c2 on (s[i+1],s[i+2])
465                // x[i-1] and x[i+2] should keep the same.
466                // (i) --- c0*(s[i+2] - s[i-1]) + c1*(s[i+2] - s[i]) + c2*(s[i+2] - s[i+1]) == 0
467                // (ii) --- c0*(s[i+2] - s[i-1])^2 + c1*(s[i+2] - s[i])^2 + c2*(s[i+2] - s[i+1])^2 == 0
468                let s0 = s[i - 1];
469                let s3 = s[i + 2];
470                let delta_s_end = (s3 - s0, s3 - s1, s3 - s2);
471                let coeff = match solve_2x2(
472                    (
473                        (delta_s_end.1, delta_s_end.2),
474                        (delta_s_end.1 * delta_s_end.1, delta_s_end.2 * delta_s_end.2),
475                    ),
476                    (-delta_s_end.0, -delta_s_end.0 * delta_s_end.0),
477                ) {
478                    Some(coeff) => {
479                        // A*[c1;c2] = b*c0
480                        coeff
481                    }
482                    None => {
483                        crate::verbosity_log!(
484                            crate::diag::Verbosity::Debug,
485                            "coeff is None? A = {:?}, b = {:?}",
486                            (
487                                (delta_s_end.1, delta_s_end.2),
488                                (delta_s_end.1 * delta_s_end.1, delta_s_end.2 * delta_s_end.2)
489                            ),
490                            (-delta_s_end.0, -delta_s_end.0 * delta_s_end.0)
491                        );
492                        flag_succeed = false;
493                        continue;
494                    }
495                };
496                // c1 = coeff.0 * c0, c2 = coeff.1 * c0
497                // Changes: a[i] += c0 * (s1-s0)^2, b[i] += c0 * (s1-s0), b[i+1] += c0 * (s2-s0) + c1 * (s2-s1)
498                let ds0 = s1 - s0;
499                let coeff_c = (ds0 * ds0, ds0, ds0 + ds1 * (1.0 + coeff.0));
500                // Changes: a[i] += c0 * coeff_c.0, b[i] += c0 * coeff_c.1, b[i+1] += c0 * coeff_c.2
501                // We hope that a(s) = a[i] + 2 * b[i] * (s - s[i]) + (b[i+1] - b[i]) / ds1 * (s - s[i])^2 >= amin holds in (s[i],s[i+1])
502                // b[i] ^ 2 * ds1 == (a[i] - amin) * (b[i+1] - b[i]) should hold for new ones.
503                // For old ones: (b[i] + coeff_c.1 * c0) ^ 2 * ds1 == (a[i] - amin + coeff_c.0 * c0) * (b[i+1] - b[i] + (coeff_c.2-coeff_c.1) * c0). Now solve c0.
504                // (coeff_c.1^2 * c0^2 + 2 * b1 * coeff_c.1 * c0 + b1 ^ 2) * ds1 == coeff_c.0 * (coeff_c.2-coeff_c.1) * c0^2 + (da * (coeff_c.2-coeff_c.1) + coeff_c.0 * db) * c0 + da * db
505                // (coeff_c.1^2 * ds1 - coeff_c.0 * (coeff_c.2-coeff_c.1)) * c0^2 + (2 * b1 * coeff_c.1 * ds1 - da * (coeff_c.2-coeff_c.1) - coeff_c.0 * db) * c0 + (b1 * b1 * ds1 - da * db) == 0
506                let coeff_solve = (
507                    coeff_c.1 * coeff_c.1 * ds1 - coeff_c.0 * (coeff_c.2 - coeff_c.1),
508                    2.0 * b1 * coeff_c.1 * ds1 - da * (coeff_c.2 - coeff_c.1) - coeff_c.0 * db,
509                    b1 * b1 * ds1 - da * db,
510                );
511                let norm = coeff_solve.0.abs() + coeff_solve.1.abs() + coeff_solve.2.abs();
512                if norm < EPS_ZERO {
513                    crate::verbosity_log!(
514                        crate::diag::Verbosity::Debug,
515                        "norm = {norm} < EPS_ZERO, coeff_solve = {coeff_solve:.8?}"
516                    );
517                    flag_succeed = false;
518                    continue;
519                }
520                let norm_inv = 1.0 / norm;
521                let coeff_solve = (
522                    coeff_solve.0 * norm_inv,
523                    coeff_solve.1 * norm_inv,
524                    coeff_solve.2 * norm_inv,
525                );
526                // coeff_solve.0 * c0^2 + coeff_solve.1 * c0 + coeff_solve.2 == 0
527                let c0 = if coeff_solve.0.abs() > EPS_ZERO {
528                    // Use quadratic formula to solve for c0
529                    let discriminant =
530                        coeff_solve.1 * coeff_solve.1 - 4.0 * coeff_solve.0 * coeff_solve.2;
531                    if discriminant < 0.0 {
532                        if coeff_c.1.abs() > EPS_ZERO && coeff_c.2.abs() > EPS_ZERO {
533                            (-b1 / coeff_c.1).min(b2 / coeff_c.2)
534                        } else if coeff_c.1.abs() > EPS_ZERO {
535                            -b1 / coeff_c.1
536                        } else if coeff_c.2.abs() > EPS_ZERO {
537                            b2 / coeff_c.2
538                        } else {
539                            crate::verbosity_log!(
540                                crate::diag::Verbosity::Debug,
541                                "discriminant = {discriminant:.8} < 0 for c0 (i={i}): coeff_solve = {coeff_solve:.8?}, coeff_c = {coeff_c:.8?}"
542                            );
543                            flag_succeed = false;
544                            continue;
545                        }
546                    } else {
547                        let sqrt_discriminant = discriminant.sqrt();
548                        // c0: (max, min)
549                        let c0 = if coeff_solve.0 > 0.0 {
550                            (
551                                (-coeff_solve.1 + sqrt_discriminant) / (2.0 * coeff_solve.0),
552                                (-coeff_solve.1 - sqrt_discriminant) / (2.0 * coeff_solve.0),
553                            )
554                        } else {
555                            (
556                                (-coeff_solve.1 - sqrt_discriminant) / (2.0 * coeff_solve.0),
557                                (-coeff_solve.1 + sqrt_discriminant) / (2.0 * coeff_solve.0),
558                            )
559                        };
560                        if c0.1 >= 0.0 { c0.1 } else { c0.0 }
561                    }
562                } else {
563                    // Linear case
564                    -coeff_solve.2 / coeff_solve.1
565                };
566                a[i] += coeff_c.0 * c0;
567                b[i] += coeff_c.1 * c0;
568                b[i + 1] += coeff_c.2 * c0;
569                a[i + 1] += (coeff_c.0 + (coeff_c.1 + coeff_c.2) * ds1) * c0;
570            }
571        }
572    }
573
574    Ok(flag_succeed)
575}
576
577/// Check the shared TOPP3 profile shape, station counts, and station ordering.
578///
579/// TOPP3/COPP3 interpolation uses node-based `a(s)` and `b(s)` profiles on the
580/// same grid, with optional stationary head/tail sections. This helper keeps
581/// those preconditions together before any timing integration is attempted.
582fn check_topp3_sab(
583    function_name: &str,
584    s: &[f64],
585    profile: Topp3ProfileRef<'_>,
586) -> Result<(), CoppError> {
587    let (a, b, num_stationary) = profile;
588    check_stationary_counts(function_name, s.len(), num_stationary)?;
589    if a.len() != s.len() || b.len() != s.len() {
590        return Err(CoppError::InvalidInput(
591            function_name.into(),
592            format!(
593                "`a.len()` = {} and `b.len()` = {} must equal `s.len()` = {}",
594                a.len(),
595                b.len(),
596                s.len()
597            ),
598        ));
599    }
600    check_input_slice_not_nan_infinite(function_name, "s", s)?;
601    check_input_slice_non_negative(function_name, "a", a)?;
602    check_input_slice_not_nan_infinite(function_name, "b", b)?;
603    check_input_strictly_increasing(function_name, "s", s)
604}
605
606/// Check that stationary head/tail counts leave at least one motion interval.
607///
608/// The minimum station count is `2 + num_stationary.0 + num_stationary.1`;
609/// checked arithmetic is used so pathological `usize` inputs are rejected as
610/// invalid input instead of overflowing.
611fn check_stationary_counts(
612    function_name: &str,
613    s_len: usize,
614    num_stationary: (usize, usize),
615) -> Result<(), CoppError> {
616    let Some(min_len) = num_stationary
617        .0
618        .checked_add(num_stationary.1)
619        .and_then(|sum| sum.checked_add(2))
620    else {
621        return Err(CoppError::InvalidInput(
622            function_name.into(),
623            "`num_stationary` overflowed while checking dimensions".into(),
624        ));
625    };
626    check_input_len_at_least(function_name, "`s.len()`", s_len, min_len)
627}
copp\copp\copp3/interpolation.rs

copp\copp\copp3/
interpolation.rs
